//
//  EvolutionaryController.cpp
//  symbrion
//
//  Created by Berend Weel on 4/15/13.
//  Copyright (c) 2013 Berend Weel. All rights reserved.
//

#include "EvolutionaryController.h"
#include "Mathlib.h"
#include "EucledianDistanceFitness.h"
#include "Logger.h"

EvolutionaryController::EvolutionaryController(WorldModel *worldModel, RobotType *robot, EAController * controller) :
    Controller(worldModel,robot),
    controller(controller), worldModel(worldModel) {
    fitnessMeasure = new EucledianDistanceFitness(worldModel);
}

EvolutionaryController::~EvolutionaryController(){
    delete fitnessMeasure;
    delete controller;
}

void EvolutionaryController::step(){
    if(state == initialising){
        initialiseEvolution();
    }else if (state == free){
        // First few seconds of the controller are not scored for fitness
        if(worldModel->time - startTime >= FREE_TIME){
            startTime = worldModel->time;
            fitnessMeasure->reset();
            
            state = evaluating;
        }
    }else{
        if(worldModel->time - startTime >= EVALUATION_TIME){
            // Evaluation done, calculate the fitness
            candidate->fitness = fitnessMeasure->calculate();
            candidate->hasFitness = true;
            
//            std::cout << worldModel->robotId << " Generation " << generation << ": evaluation " << evaluations << " done: " << candidate->fitness << std::endl;
            Logger::getFitnessLogger().info(logMessage(worldModel->robotId.c_str(), " Generation ", generation, ": evaluation ", evaluations, " done: ", candidate->fitness));
            if(initialEvaluation || (candidate->id != population[0].id && candidate->fitness > population[0].fitness)){
                // This one is better than the worst one (or was in the initial population).
                if(!initialEvaluation){
                    // replace the worst in the population
                    population.pop_front();
                    population.push_front(candidate);
                }
                
                // Set the news for this candidate
                Individual *individual = new Individual(*candidate);
                News *news = new News(worldModel->time,individual);
                setNews(news);
                
                // Sort the population
                population.sort(individualCompare);
                
                // Check if its the champion
                if(candidate->id != best.id && candidate->fitness > best.fitness){
                    best = *candidate;
                }
            }
            
            // Get the next candidate
            startEvaluation();
        }
    }
    
    
    // Let the controller control the robot
    controller->step();
    
    // Exchange the cache with others
    exchangeCaches();
}

void EvolutionaryController::initialiseEvolution(){
    // Initialise the population with random individuals
    while(population.size() < POPULATION_SIZE){
        population.push_back(controller->randomIndividual());
    }
    
    generation = 0;
    evaluations = 0;
    
    best = population.front();
    
    startEvaluation();
}

void EvolutionaryController::startEvaluation(){
    // See if there are any in the population that have no fitness
    boost::ptr_deque<Individual>::iterator it = std::find_if(population.begin(), population.end(), EvolutionaryController::hasNoFitness);
    if(it != population.end()){
        candidate = &(*it);
        initialEvaluation = true;
    }else{
        initialEvaluation = false;
        candidate = getNextCandidate();
    }
    
    state = free;
    
    // Load the new genome
    controller->setGenome(candidate->genome);
    
    evaluations++;
    
    startTime = worldModel->time;
}

Individual* EvolutionaryController::getNextCandidate(){
    Individual *next = new Individual();;
    
    Individual *father = uniformSelection();
    if(Mathlib::getBool(CROSSOVERRATE)){
        Individual *mother = uniformSelection();
        uniformCrossOver(next,father,mother);
    }else{
        next->copy(*father);
    }
    
    gaussianMutation(next);
    
    generation++;
    
    return next;
}

Individual* EvolutionaryController::uniformSelection(){
    // Select a uniform random individual from the news cache
    int choice = Mathlib::getInt((int)cache.size());
    Individual *selected = cache[choice].getIndividual();
    
    return selected;
}

void EvolutionaryController::uniformCrossOver(Individual *next,Individual *father,Individual *mother){
    // Implements a unifrom crossover
    // Genes are picked from either the father or the mother
    next->genome.clear();

    
    if(Mathlib::getBool()){
        next->sigma = father->sigma;
    }else{
        next->sigma = mother->sigma;
    }
    
    for(size_t i=0;i<father->genome.size();i++){
        if(Mathlib::getBool()){
            next->genome.push_back(father->genome[i]);
        }else{
            next->genome.push_back(mother->genome[i]);
        }
    }
}

void EvolutionaryController::gaussianMutation(Individual *next) {
    // Implements a gaussian mutation with a single mutation step-size
    LIOReal learningRate = 1 / std::sqrt(next->genome.size());
    LIOReal minSigma = 0.001;
    
    LIOReal N = Mathlib::getGaussianRand(0, 1);
    next->sigma = next->sigma * std::exp(learningRate * N);
    next->sigma = next->sigma < minSigma ? minSigma : next->sigma;
    
    for (int i=0;i<next->genome.size();i++){
        next->genome[i] = next->genome[i] + Mathlib::getGaussianRand(0,next->sigma);
    }
}

void EvolutionaryController::setNews(News *news){
    if(cache.size() >= MAX_CACHE_SIZE){
        cache.pop_back();
        cache.push_front(news);
    }else{
        cache.push_front(news);
    }
    // Cache should still be sorted
//    cache.sort(newsGreaterThan);
}

void EvolutionaryController::exchangeCaches(){
    // Send our cache to others
    std::ostringstream cacheString;
    cacheString << "cache:";
    for(int i = 0; i < cache.size(); i++){
        // Overriden operator<< outputs the cache in the correct format
        cacheString << cache[i] <<  ";";
    }
    std::string final = cacheString.str();
    worldModel->radioMsgToSend.push_back(new Message("*",final));

    // Unify the cache with the received ones
    receiveCaches();
    if(cache.size() > 0){
        cache.sort(newsGreaterThan);
        cache.unique();
    }
    
    // Only keep the newest N
    if(cache.size() > MAX_CACHE_SIZE){
        cache.erase(cache.begin()+MAX_CACHE_SIZE, cache.end());
    }
}

void EvolutionaryController::receiveCaches(){
    // This function adds all news items received from messages to the cache
    for(int i=0;i<worldModel->radioMsgReceived.size();i++){
        std::string msg = worldModel->radioMsgReceived[i].getDataString();
        if(msg.find("cache:") != std::string::npos){
            // Extract the individual cache elements
            std::string cachestring = msg.substr(msg.find(":")+1,std::string::npos);
            std::vector<std::string> items;
            size_t pos;
            while((pos = cachestring.find(";")) != std::string::npos){
                items.push_back(cachestring.substr(0,pos));
                cachestring.erase(0,pos+1);
            }
            
            for(int i=0;i<items.size();i++){
                // Special constructor parses cache description
                News *item = new News(items[i]);
                cache.push_back(item);
            }
        }
    }
}
